#!/usr/bin/env python
# coding:utf-8
import socket
import rospy
import os
from std_msgs.msg import Int8
import json

running_node = 0

def callback(data):
    global running_node
    running_node = data.data

def launch_node(node_name,arg=''):
    global running_node
    if running_node:
        rospy.loginfo("A node is already running")
        return
    # Here, launch the node
    os.system('rosrun'+' '+node_name+' '+arg)
    rospy.loginfo("Launching node: {}".format(node_name))

def tcp_server():
    HOST = ''  # Listen on all available interfaces
    PORT = 11451  # Arbitrary non-privileged port

    with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
        s.bind((HOST, PORT))
        s.listen()
        rospy.loginfo("Waiting for connections on port {}".format(PORT))
        while not rospy.is_shutdown():
            conn, addr = s.accept()
            with conn:
                rospy.loginfo("Connected by: {}".format(addr))
                data = conn.recv(1024).decode('utf-8')
                json_string=json.loads(data) # json对数据进行解析，得到dict数据
                order_name = json_string['order_name_name']
                order_args = json_string['order_args']
                if running_node==0 and order_name == "ORDER_TAKE_OFF":
                    launch_node("offboard_pkg offboard_node",order_args)


if __name__ == '__main__':
    try:
        #启动话题程序锁，防止同时运行两个脚本
        rospy.init_node('tcp_server')
        rospy.Subscriber("IS_RUNNING_TOPIC", Int8, callback)
        RUNNING_LOCK_PUB = rospy.Publisher('IS_RUNNING_TOPIC',Int8)
        RUNNING_LOCK_PUB.publish(0)
        #启动服务器
        tcp_server()
    except rospy.ROSInterruptException:
        pass
